#!/usr/bin/env python3
# -*-coding:utf8-*-
# 获取机械臂所有关节的位置和速度示例
from piper_sdk import *
import time

def get_joint_position(piper):
    """
    获取所有关节的位置
    
    Args:
        piper: 机械臂接口实例
        
    Returns:
        positions: 各关节位置（单位：度）
    """
    # 获取关节位置
    joint_msgs = piper.GetArmJointMsgs()
    
    # 提取位置信息（单位：度）
    # 根据检查结果，joint_msgs有joint_state属性，它是一个ArmMsgJointFeedBack对象
    positions = {
        "joint_1": joint_msgs.joint_state.joint_1 * 0.001,
        "joint_2": joint_msgs.joint_state.joint_2 * 0.001,
        "joint_3": joint_msgs.joint_state.joint_3 * 0.001,
        "joint_4": joint_msgs.joint_state.joint_4 * 0.001,
        "joint_5": joint_msgs.joint_state.joint_5 * 0.001,
        "joint_6": joint_msgs.joint_state.joint_6 * 0.001
    }
    
    return positions

def calculate_joint_velocity(piper, interval=0.05):
    """
    通过计算两个时间点的位置差来估算关节速度
    
    Args:
        piper: 机械臂接口实例
        interval: 采样时间间隔（秒）
        
    Returns:
        velocities: 各关节速度（单位：度/秒）
    """
    # 第一次读取位置
    pos1 = get_joint_position(piper)
    
    # 等待一小段时间
    time.sleep(interval)
    
    # 第二次读取位置
    pos2 = get_joint_position(piper)
    
    # 计算速度（度/秒）
    velocities = {
        "joint_1": (pos2["joint_1"] - pos1["joint_1"]) / interval,
        "joint_2": (pos2["joint_2"] - pos1["joint_2"]) / interval,
        "joint_3": (pos2["joint_3"] - pos1["joint_3"]) / interval,
        "joint_4": (pos2["joint_4"] - pos1["joint_4"]) / interval,
        "joint_5": (pos2["joint_5"] - pos1["joint_5"]) / interval,
        "joint_6": (pos2["joint_6"] - pos1["joint_6"]) / interval
    }
    
    return velocities

def print_joint_info(positions, velocities):
    """打印关节位置和速度信息"""
    print("=" * 50)
    print("关节位置和速度信息：")
    print("-" * 50)
    for i in range(1, 7):
        joint_key = f"joint_{i}"
        print(f"关节 {i}:")
        print(f"  位置: {positions[joint_key]:.3f} 度")
        print(f"  速度: {velocities[joint_key]:.3f} 度/秒")
    print("=" * 50)

if __name__ == "__main__":
    # 创建机械臂接口实例
    piper = C_PiperInterface_V2(can_name="right")
    
    # 连接机械臂
    piper.ConnectPort()
    print("机械臂已连接")
    
    try:
        # 循环获取关节位置和速度
        while True:
            # 获取关节位置
            positions = get_joint_position(piper)
            
            # 计算关节速度
            velocities = calculate_joint_velocity(piper, interval=0.05)
            
            # 打印信息
            print_joint_info(positions, velocities)
            
            # 等待一段时间
            time.sleep(0.5)
    except KeyboardInterrupt:
        print("程序已终止")
    finally:
        # 断开连接
        piper.DisconnectPort()
        print("机械臂已断开连接")
